/* @author Stone, at Midea and GDOU
   @date March to May, 2021
   @brief common definitions about BLDC Driver
*/
#include "BLDCCommon.h"
#include <Arduino.h>
#include <TimerOne.h>


/* Hardware definition specified to my Board: BLDC Driver v2, Graudation Project.

    INH_x and IN_x are about the pins for output to BTN8982,
    HALL_x are about the pins for input from hall sensors.
 */
const int INH[] = {INH_A, INH_B, INH_C};
const int IN[]  = {IN_A, IN_B, IN_C};
const int HALL[] = {HALL_A, HALL_B, HALL_C};


/* ==================
   Map from hall code to step code
   ==================*/

const byte halls[6][3] = {
  {0,0,1},  
  {0,1,1},
  {0,1,0},
  {1,1,0},
  {1,0,0},
  {1,0,1}
};


const byte steps[6][6] = {  
  // INH_A, IN_A, INH_B, IN_B, INH_C, IN_C
  {1,1,1,0,0,0},  // AB
  {1,1,0,0,1,0},  // AC
  {0,0,1,1,1,0},  // BC
  {1,0,1,1,0,0},  // BA
  {1,0,0,0,1,1},  // CA
  {0,0,1,0,1,1}  // CB
};


// ===== ===== ===== ===== =====
// Local variables inside this module
// ===== ===== ===== ===== =====
// TODO use a struct to integrate them
static byte inh[3], in[3];
static MotorDir dir = MotorForward;
static uint16_t pwm_duty = 0;
static float g_speed = 0;



// ===== ===== ===== ===== ==== =====
// Declarations of internal functions
// ===== ===== ===== ===== ==== =====
static void BLDCSet(const byte inh[], const byte in[]);
static int NextStep(byte inh[], byte in[], MotorDir dir=1);
static float CalcSpeed(int idx);


// ===== ===== ===== ===== ==== =====
//  Implementations
// ===== ===== ===== ===== ==== =====
/* Initialize the pin mode and pwm settings*/
void BLDC_Init()
{
  for (int i=0; i<3; i++){
    pinMode(INH[i], OUTPUT);
    digitalWrite(INH[i], LOW);  // disable BTN8982

    pinMode(IN[i], OUTPUT);
    digitalWrite(INH[i], LOW);
    
    pinMode(HALL[i], INPUT_PULLUP);
  }
  pinMode(I_A, INPUT);
  pinMode(I_B, INPUT);
  pinMode(I_C, INPUT);
  pinMode(V_SYS_PIN, INPUT);
  Timer1.initialize(PWM_PERIOD);
  Timer1.pwm(PWM_PIN, pwm_duty);
}


/* @brief Set the direction of motor
*/
void BLDC_SetDir(MotorDir new_dir) {
    dir = new_dir;
}


/* @brief Set the speed of BLDC, currently the speed is pwm duty
 */
void BLDC_SetPwmDuty(uint16_t duty) {
    if (duty > 300) {return;}  // for safe reason
    Timer1.setPwmDuty(PWM_PIN, duty);
}

float BLDC_ReadSpeed() {
    return g_speed;
}

void BLDC_Update() {
    int idx = NextStep(inh, in, dir);
    BLDCSet(inh, in);
    CalcSpeed(idx);
}


/* Return the voltage of the system power. */
float BLDC_ReadSysVolt() {
    // value of voltage divider: 6.8k, 1k
    return (float)analogRead(V_SYS_PIN) / 1024.0 * 5.0 * 7.8;
}


/* Read the current of each phase. */
void BLDC_ReadCurrent(uint16_t *ia, uint16_t *ib, uint16_t *ic) {
    *ia = analogRead(I_A);
    *ib = analogRead(I_B);
    *ic = analogRead(I_C);
}


/* Output the signal of this step */
static void BLDCSet(const byte inh[], const byte in[]){
  for (int i=0; i<3; i++){
    digitalWrite(INH[i], inh[i]);
    digitalWrite(IN[i], in[i]);
  }
}


static int NextStep(byte inh[], byte in[], MotorDir dir=1) {
  int idx = -1;
  byte a,b,c;
  
  a = digitalRead(HALL_A);
  b = digitalRead(HALL_B);
  c = digitalRead(HALL_C);
  for (int i=0; i<6; i++) {
    if (   a == halls[i][0]
        && b == halls[i][1]
        && c == halls[i][2]) {
      idx = i;
      break;
    }
  }

  if (idx<0 || idx>5) {return -1;}  // no match

  if (dir == MotorBackward)  {idx = (idx + 3) % 6;}
  
  // copy state of current
  for (int i=0; i<3; i++){
    inh[i] = steps[idx][2*i];
    in[i] = steps[idx][2*i+1];
  }

  return idx;
}


static float CalcSpeed(int idx) {
    static int _last_idx = idx;
    static unsigned long _last_t = 0, _this_t = 0;
    static float _count = 0;
    static float _avg_speed = 0, _inst_speed = 0;  // average speed, instant speed
    
    // time
    _this_t = micros();
    if (_this_t < _last_t) {  // overflow
        _last_t = -10;
    }
    else if (_this_t == _last_t) {
        return;
    }
    float _dt = _this_t - _last_t;
    
    
    // count step
    if (idx == _last_idx && _dt < 1000000L) {
        // step not switched in 1 second, ignore it
        return;
    }
    
    
    if (MotorForward == dir) {
        if (idx >= _last_idx) {
            _count = idx - _last_idx;
        }
        else {
            _count = idx + 1;
        }
    }
    else if (MotorBackward == dir) {
        if (idx <= _last_idx) {
            _count = _last_idx - idx;
        }
        else {
            _count = 6 - idx;
        }
    }
    _last_idx = idx;
    _last_t = _this_t;
    
    // speed
    // speed(Hz) = (count / 36) / (dt / 1000000) = count / dt * 16666.67
    _inst_speed = 27777.78 * _count / _dt;
    _avg_speed = 0.2*_inst_speed + 0.8*_avg_speed;
    
    g_speed = _avg_speed;
}